#ifndef _COMMAND_H_
#define _COMMAND_H_

#include "motor.h"
#include <EEPROM.h>

struct command {
    int length;
    char buff[64];
} cmd;

int pos;

inline void save () {
    int pos = 0;
    EEPROM.write (pos ++, conf.left_pwm);
    EEPROM.write (pos ++, conf.right_pwm);
    EEPROM.write (pos ++, conf.turn_left_time >> 8);
    EEPROM.write (pos ++, conf.turn_left_time & 0xff);
    EEPROM.write (pos ++, conf.turn_right_time >> 8);
    EEPROM.write (pos ++, conf.turn_right_time & 0xff);
    EEPROM.write (pos ++, conf.speed_time >> 8);
    EEPROM.write (pos ++, conf.speed_time & 0xff);
    Serial.println ("config data saved.");
}

inline void show_config () {
    Serial.print ("left_pwm        = ");
    Serial.println (conf.left_pwm);
    Serial.print ("right_pwm       = ");
    Serial.println (conf.right_pwm);
    Serial.print ("turn_left_time  = ");
    Serial.print (conf.turn_left_time);
    Serial.println (" ms.");
    Serial.print ("turn_right_time = ");
    Serial.print (conf.turn_right_time);
    Serial.println (" ms.");
    Serial.print ("speed time      = ");
    Serial.print (conf.speed_time);
    Serial.println (" ms.");
}

inline void read_config () {
    int pos = 0;
    conf.left_pwm        = EEPROM.read (pos ++);
    conf.right_pwm       = EEPROM.read (pos ++);
    conf.turn_left_time  = ((uint16_t)EEPROM.read (pos ++)) << 8 | EEPROM.read (pos ++);
    conf.turn_right_time = ((uint16_t)EEPROM.read (pos ++)) << 8 | EEPROM.read (pos ++);
    conf.speed_time      = ((uint16_t)EEPROM.read (pos ++)) << 8 | EEPROM.read (pos ++);
    show_config ();
}

inline void show_left_pwm () {
    Serial.print ("left_pwm  = ");
    Serial.println (conf.left_pwm);
}

inline void show_right_pwm () {
    Serial.print ("right_pwm  = ");
    Serial.println (conf.right_pwm);
}

inline void show_state () {
    Serial.print ("state      = ");
    Serial.println (conf.state);
}

inline void show_turn_left_time () {
    Serial.print ("turn_left_time  = ");
    Serial.println (conf.turn_left_time);
}

inline void show_turn_right_time () {
    Serial.print ("turn_right_time  = ");
    Serial.println (conf.turn_right_time);
}

inline void show_speed_time () {
    Serial.print ("speed_time = ");
    Serial.println (conf.speed_time);
}

void process_command (struct command *cmd) {
/*
    Serial.print ("receive command: ");
    Serial.println (cmd->buff);
*/
    String str = String (cmd->buff);
    String key, value;
    
    int pos = str.indexOf ("=");
    
    if (pos != -1) {
        key  = str.substring (0, pos);
        value = str.substring (pos + 1);
    } else {
        key = str;
    }
/*    
    Serial.print ("key = ");
    Serial.print (key);
    Serial.print (", value = ");
    Serial.println (value);
*/
    key.trim ();
    if (key == "?" && pos == -1) {
        // show all configs
        Serial.println ("OK, ");
        show_config ();
    } else if (key == "left pwm") {
        if (pos != -1) {
            conf.left_pwm = atoi (value.c_str());
        }
        Serial.print ("OK, ");
        show_left_pwm ();
    } else if (key == "right pwm") {
        if (pos != -1) {
            conf.right_pwm = atoi (value.c_str());
        }
        Serial.print ("OK, ");
        show_right_pwm ();
    } else if (key == "left time") {
        if (pos != -1) {
            conf.turn_left_time = atoi (value.c_str ());
        }
        Serial.print ("OK, ");
        show_turn_left_time ();
    } else if (key == "right time") {
        if (pos != -1) {
            conf.turn_right_time = atoi (value.c_str ());
        }
        Serial.print ("OK, ");
        show_turn_right_time ();
    } else if (key == "speed time") {
        if (pos != -1) {
            conf.speed_time = atoi (value.c_str ());
        }
        Serial.print ("OK, ");
        show_speed_time ();
    } else if (key == "left forward") {
        pwm_forward (&m0, conf.left_pwm);
    } else if (key == "right forward") {
        pwm_forward (&m1, conf.right_pwm);
    } else if (key == "forward") {
        int n = 0;
        if (pos != -1) {
            n = atoi (value.c_str ());
/*
            Serial.print ("forward ");
            Serial.print (n);
            Serial.println (" ms.");
*/
        }
        pwm_forward (&m0, conf.left_pwm);
        pwm_forward (&m1, conf.right_pwm);
        conf.state = SC_FORWARD;
        
        if (n) {
            delay (n);
            pause ();
        }
    } else if (key == "test") {
        digitalWrite (13, LOW);
        analogWrite (11, 100);
    } else if (key == "left backward") {
        pwm_backward (&m0, conf.left_pwm);
    } else if (key == "right backward") {
        pwm_backward (&m1, conf.right_pwm);
    } else if (key == "backward") {
        pwm_backward (&m0, conf.left_pwm);
        pwm_backward (&m1, conf.right_pwm);
        int n = 0;
        if (pos != -1) {
            n = atoi (value.c_str ());
/*
            Serial.print ("backward ");
            Serial.print (n);
            Serial.println (" ms.");
*/
        }
        conf.state = SC_BACKWARD;
        if (n) {
            delay (n);
            pause ();
        }
    } else if (key == "turn left") {
        pwm_backward (&m0, conf.left_pwm);
        pwm_forward (&m1, conf.right_pwm);
        delay (conf.turn_left_time);
    } else if (key == "turn right") {
        pwm_forward (&m0, conf.left_pwm);
        pwm_backward (&m1, conf.right_pwm);
        delay (conf.turn_right_time);
    } else if (key == "stop") {
        pause ();
        conf.state = SC_STOPPED;
    } else if (key == "read") {
        read_config ();
    } else if (key == "save") {
        save ();
    } else if (key.startsWith ("forward ")) {
        value = key.substring (8);
        value.trim ();
        int n = atoi (value.c_str ());
        pwm_forward (&m0, conf.left_pwm);
        pwm_forward (&m1, conf.right_pwm);
        conf.state = SC_FORWARD;
        delay (n);
    } else {
        Serial.print ("Unknown command: ");
        Serial.println (key);
    }
}

inline void read_command (Stream *in) {
    while (in->available ()) {
        int ch = in->read ();
        if (ch != -1) {
            cmd.buff [pos ++] = ch;

            if (ch == '\n' && cmd.buff [pos - 2] == '\r') {
                cmd.length = pos - 1;
                if (cmd.length > 1) {
                    cmd.buff [cmd.length - 1] = '\0';
                    process_command (&cmd);
                }
                memset (cmd.buff, 0, 64);
                cmd.length = 0;
                pos = 0;
            }
        }
    }
}
#endif // _COMMAND_H_
